import geomeas as gm
import numpy as np

# 输入光平面上三个点坐标
Point1 = np.array([293.23562748, -0.42708872, 2759.95577537])
Point2 = np.array([608.91301696, -0.42708872, 1000])
Point3 = np.array([-59.43212107, -0.42708872, 1000])

# 输入相机光心坐标
O_c = np.array([203.69706481, -140.24425530, 2914.03048234])

# 输入盾尾间隙左右特征点投影坐标
P_l1 = np.array([183.41200579, -159.73768606, 3180.88627410])
P_r1 = np.array([197.49395955, -159.73768606, 3180.88627410])

# 计算光平面方程的参数
a, b, c, d = gm.Plane().calPlaneFrom3Points(Point1, Point2, Point3)
LightPlaneParam = np.array([a, b, c, d])

# 计算左右投影直线与光平面的交点，即盾尾间隙特征点坐标
P_l = gm.Coordinate().calCoordinateFrom2PointsAndPlane(O_c, P_l1, LightPlaneParam)
P_r = gm.Coordinate().calCoordinateFrom2PointsAndPlane(O_c, P_r1, LightPlaneParam)

# 计算两特征点的距离
Distance = gm.Distance().calDistanceFrom2Points(P_r, P_l)

print('ShieldTailClearance:', Distance)
